#include <chrono>
#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/nav_sat_fix.hpp"

using std::placeholders::_1;
using namespace std::chrono_literals;

class KalmanFilter
{
public:
    KalmanFilter(double process_noise, double measure_noise, double estimate_error, double init_value)
    {
        Q = process_noise;
        R = measure_noise;
        P = estimate_error;
        X = init_value;
        initialized = false;
    }

    double filter(double measurement)
    {
        if (!initialized)
        {
            X = measurement;
            initialized = true;
        }
        P = P + Q;
        double K = P / (P + R);
        X = X + K * (measurement - X);
        P = (1 - K) * P;
        return X;
    }

    void reset(double value)
    {
        X = value;
        initialized = true;
    }

    void update_params(double process_noise, double measure_noise, double estimate_error)
    {
        Q = process_noise;
        R = measure_noise;
        P = estimate_error;
    }

private:
    double Q, R, P, X;
    bool initialized;
};

class GpsKalmanFilterNode : public rclcpp::Node
{
public:
    GpsKalmanFilterNode()
        : Node("gps_kalman_filter_node")
    {
        // 声明参数（可通过启动参数或动态设置）
        declare_parameter("lat.process_noise", 1e-7);
        declare_parameter("lat.measure_noise", 1e-6);
        declare_parameter("lat.estimate_error", 1.0);
        declare_parameter("lat.init_value", 0.0);

        declare_parameter("lon.process_noise", 1e-7);
        declare_parameter("lon.measure_noise", 1e-6);
        declare_parameter("lon.estimate_error", 1.0);
        declare_parameter("lon.init_value", 0.0);

        declare_parameter("alt.process_noise", 1e-4);
        declare_parameter("alt.measure_noise", 1e-2);
        declare_parameter("alt.estimate_error", 1.0);
        declare_parameter("alt.init_value", 0.0);

        // 初始化滤波器
        lat_filter = std::make_unique<KalmanFilter>(
            get_parameter("lat.process_noise").as_double(),
            get_parameter("lat.measure_noise").as_double(),
            get_parameter("lat.estimate_error").as_double(),
            get_parameter("lat.init_value").as_double());

        lon_filter = std::make_unique<KalmanFilter>(
            get_parameter("lon.process_noise").as_double(),
            get_parameter("lon.measure_noise").as_double(),
            get_parameter("lon.estimate_error").as_double(),
            get_parameter("lon.init_value").as_double());

        alt_filter = std::make_unique<KalmanFilter>(
            get_parameter("alt.process_noise").as_double(),
            get_parameter("alt.measure_noise").as_double(),
            get_parameter("alt.estimate_error").as_double(),
            get_parameter("alt.init_value").as_double());

        // 支持动态参数修改
        parameter_callback_handle_ = this->add_on_set_parameters_callback(
            std::bind(&GpsKalmanFilterNode::parameters_callback, this, std::placeholders::_1));

        // 订阅与发布
        subscription_ = this->create_subscription<sensor_msgs::msg::NavSatFix>(
            "/fix", 10, std::bind(&GpsKalmanFilterNode::topic_callback, this, _1));
        publisher_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("/fix/filter", 10);

        RCLCPP_INFO(this->get_logger(), "GPS卡尔曼滤波节点已启动（参数可配置）");
        // 输出当前滤波器参数
        RCLCPP_INFO(this->get_logger(), "纬度滤波器参数: Q=%.8f, R=%.8f, P=%.8f, 初始值=%.8f",
                    get_parameter("lat.process_noise").as_double(),
                    get_parameter("lat.measure_noise").as_double(),
                    get_parameter("lat.estimate_error").as_double(),
                    get_parameter("lat.init_value").as_double());
    }

private:
    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;

    rclcpp::Subscription<sensor_msgs::msg::NavSatFix>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr publisher_;
    std::unique_ptr<KalmanFilter> lat_filter;
    std::unique_ptr<KalmanFilter> lon_filter;
    std::unique_ptr<KalmanFilter> alt_filter;

    // 参数回调，支持动态调整滤波器参数
    rcl_interfaces::msg::SetParametersResult parameters_callback(
        const std::vector<rclcpp::Parameter> &params)
    {
        for (const auto &param : params)
        {
            if (param.get_name() == "lat.process_noise" ||
                param.get_name() == "lat.measure_noise" ||
                param.get_name() == "lat.estimate_error")
            {
                lat_filter->update_params(
                    get_parameter("lat.process_noise").as_double(),
                    get_parameter("lat.measure_noise").as_double(),
                    get_parameter("lat.estimate_error").as_double());
            }
            if (param.get_name() == "lon.process_noise" ||
                param.get_name() == "lon.measure_noise" ||
                param.get_name() == "lon.estimate_error")
            {
                lon_filter->update_params(
                    get_parameter("lon.process_noise").as_double(),
                    get_parameter("lon.measure_noise").as_double(),
                    get_parameter("lon.estimate_error").as_double());
            }
            if (param.get_name() == "alt.process_noise" ||
                param.get_name() == "alt.measure_noise" ||
                param.get_name() == "alt.estimate_error")
            {
                alt_filter->update_params(
                    get_parameter("alt.process_noise").as_double(),
                    get_parameter("alt.measure_noise").as_double(),
                    get_parameter("alt.estimate_error").as_double());
            }
        }
        rcl_interfaces::msg::SetParametersResult result;
        result.successful = true;
        result.reason = "参数更新成功";
        RCLCPP_INFO(this->get_logger(), "参数更新成功");
        return result;
    }

    void topic_callback(const sensor_msgs::msg::NavSatFix::SharedPtr msg)
    {
        if (std::isnan(msg->latitude) || std::isnan(msg->longitude) || std::isnan(msg->altitude) || msg->status.status < 0)
        {
            RCLCPP_WARN(this->get_logger(), "收到无效GPS数据，滤波器不处理此数据");
            return;
        }

        double filtered_lat = lat_filter->filter(msg->latitude);
        double filtered_lon = lon_filter->filter(msg->longitude);
        double filtered_alt = alt_filter->filter(msg->altitude);

        auto filtered_msg = sensor_msgs::msg::NavSatFix();
        filtered_msg.header = msg->header;
        filtered_msg.status = msg->status;
        filtered_msg.latitude = filtered_lat;
        filtered_msg.longitude = filtered_lon;
        filtered_msg.altitude = filtered_alt;
        filtered_msg.position_covariance = msg->position_covariance;
        filtered_msg.position_covariance_type = msg->position_covariance_type;

        publisher_->publish(filtered_msg);

        RCLCPP_INFO(this->get_logger(), "原始GPS: [纬度: %.8f, 经度: %.8f, 高度: %.3f]", msg->latitude, msg->longitude, msg->altitude);
        RCLCPP_INFO(this->get_logger(), "滤波后GPS: [纬度: %.8f, 经度: %.8f, 高度: %.3f]", filtered_lat, filtered_lon, filtered_alt);
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<GpsKalmanFilterNode>());
    rclcpp::shutdown();
    return 0;
}

